// 尝试将imu和lidar时间戳进行对齐
#include <iostream>
#include <vector>
#include <queue>

#include <mutex>

struct imu
{
    int data = 0;
    double time_stamp = 0.0;
};

struct point
{
    int data = 0;
    double time_stamp = 0.0;
};

struct point_cloud
{
    std::vector<point> points;
    double time_stamp = 0.0;
};

struct data
{
    std::queue<imu> q_imu;
    point_cloud cloud;
    double strat_time = 0;
    double end_time = 0;
};

// 假设这里进行了很多处理，产生了很多imu和point_cloud数据。
std::queue<imu> q_imu;
std::queue<point_cloud> q_point_cloud;

std::mutex data_mutex;

// 对齐imu和整个雷达数据，加锁防止多线程数据冲突
bool get_align_data(data out){
    data_mutex.lock();
    if(q_point_cloud.size() < 1) return false;  // 没有得到一个完整的点云帧
    if(q_point_cloud.front().points.back().time_stamp > q_imu.back().time_stamp)  return false; // imu数据不够
    
    // 下面开始对齐帧
    while (q_imu.front().time_stamp < q_point_cloud.front().time_stamp)
    {
        q_imu.pop();    // 清除多余的
    }
    out.cloud = q_point_cloud.front();
    out.strat_time = q_point_cloud.front().time_stamp;
    out.end_time = q_point_cloud.front().points.back().time_stamp;
    while (q_imu.front().time_stamp < out.end_time)
    {
        out.q_imu.push(q_imu.front());
        q_imu.pop();
    }
    q_point_cloud.pop();
    data_mutex.unlock();
    return true;
}



int main(int argc, char* argv[]){

    std::cout << "ok" << std::endl;
    return 0;
}
